/* EasyCODE V8 */
/* EasyCODE ( 0 
dsESC2.5 */
/* EasyCODE ( 0 
Define */
/* EasyCODE ( 0 
Define */
#include <p33FJ16GS504.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "JI2C.h"
#include "JUART.h"
#include "openloop.h"


#include "init.h"


/* Configuration Bit Settings */
_FOSCSEL(FNOSC_PRIPLL)
_FOSC(POSCMD_XT & OSCIOFNC_ON)
_FPOR(FPWRT_PWR128 )
_FICD(ICS_PGD3 & JTAGEN_OFF)
_FWDT(FWDTEN_OFF)

/* EasyCODE ) */
/* EasyCODE ( 0 
Variables */
unsigned char   _i2cAddress=0;
unsigned int    _i2cTimeout=0;
unsigned char   _i2cState=0;

unsigned int _BEMF_A=0;
unsigned int _BEMF_B=0;
unsigned int _BEMF_C=0;
unsigned int _BEMF_BUS=0;
#define _BEMF_BUS_LOW 10

unsigned int _CURRENT=0;
#define CURRENT_LIMIT 15   // AMPS
#define CURRENT_LIMIT_DAC   ((CURRENT_LIMIT*0.02f)/(1.2f/1024.0f))
#define POLES  12


unsigned char DEMAG=0;
unsigned int DEMAGtime=0;


unsigned char BEMFMODE=0;
unsigned int BEMFSTARTCOUNT=0;
unsigned char state=0;
unsigned char Step=0;
unsigned int delay=0;
unsigned char Align=0;
unsigned int errorcount=0;
unsigned int errorthreshold=0;

unsigned int PIDStartCounter=0;
unsigned int PIDStartDelay=0;


unsigned int RPMpulse=0;
float RPMcalc=0;
signed int RPM=0;

signed int MinSetpoint=250;
signed int Setpoint=0;
signed int SetpointOld=0;
signed int error=0;
unsigned char startcalculation=0;

unsigned int PIDset=0;
unsigned int oldPIDset=0;
float PID=0;
float PIDold=0;
float Pvalue=0;
float Ivalue=0;
float Dvalue=0;
float limit=23576;
float ratelimit=0;

float Kp=0;
float Ti=0;
float Ki=0;
float Kd=0;
float dt=0;



unsigned long TIMER3ZC=0;
unsigned long TIMER3C=0;
unsigned long CDELAY=0;

unsigned int PR3Arr[8];
unsigned int PR3ArrPos = 0;
unsigned long PR3value=0;


//2nd order 60Hz (fs=500Hz)
float a0=0.100960;
float a1=0.201920;
float a2=0.100960;
float b1=-1.086170;
float b2=0.490009;

void PID_loop(void);

float biquad(float in);

int main(void)
{
    InitPort();
    InitCLOCK();
    InitPWM();
    InitADC();
    InitDAC();
    //InitI2C();
    InitUART1();
    InitINT();
    _POR=0;
    _BOR=0;
    
    MAA=0;
    MBA=0;
    MCA=0;
    Setpoint=8000;
    /* EasyCODE - */
    /* EasyCODE - */
    _U1RXIE=1;
    _U1RXIF=0;
    /* EasyCODE - */
    Kp=0.1;	//Kp
    Ki=1;  //Ti
    Kd=0; 	//Td
    /* EasyCODE - */
    PIDStartDelay=30000;

    /* EasyCODE - */
    CMPDAC4bits.CMREF= CURRENT_LIMIT_DAC; //=(12A*0.02)/(1.2V/1024)
    GLED=0;
    OLED=0;
    RLED=0;
    /* EasyCODE - */
    T1CONbits.TON=1;
    T2CONbits.TON=1;
    T3CONbits.TON=1;
    PTCONbits.PTEN=1;
    while ( 1 )
    {
        if ( startcalculation==1 )
        {
            Setpoint=400;
            if ( Setpoint>=MinSetpoint )
            {
                /* EasyCODE ( 0 
                RPM Calculation */
                RPMcalc=RPMpulse;
                RPMcalc=biquad(RPMcalc);
                if ( RPMcalc<10000.0f )
                {
                    RPM=(signed int)((1/(0.0000016*RPMcalc*6*(POLES/2.0f)))*60);
                    //CMPDAC1bits.CMREF= ((unsigned int)(RPMcalc/6.3477));
                }
                else
                {
                    RPMcalc=0;
                }
                /* EasyCODE ) */
                if ( PIDStartCounter>=(PIDStartDelay/2) )
                {
                    /* EasyCODE ( 0 
                    OpenLoop */
                    PID=Setpoint*10;
                    if ( PID>23000 )
                    {
                        PID=23000;
                    }
                    else
                    {
                        if ( PID<500 )
                        {
                            PID=500;
                        }
                    }
                    PIDset=(unsigned int)PID;
                    /* EasyCODE ) */
                }
            }
            else
            {
                Pvalue=0;
                Ivalue=0;
                PID=0;
                PIDset=0;
                RPMpulse=0;
                RPM=0;
                PIDStartCounter=0;
                state=0;
            }
            startcalculation=0;
            TIMER=0;
        }
    }
}

/* EasyCODE ) */
/* EasyCODE ( 0 
void PID(void) */
/* EasyCODE F */
void PID_loop(void)
{
    if ( Setpoint>=MinSetpoint )
    {
        error=(Setpoint-RPM);
        if ( error>3000 )
        {
            error=3000;
        }
        else
        {
            if ( error<-3000 )
            {
                error=-3000;
            }
        }
        //P-factor
        Pvalue=error*Kp;
        //I-factor
        Ivalue=Ivalue+(error*dt*Ki);
        if ( Ivalue>100 )
        {
            Ivalue=100;
        }
        else
        {
            if ( Ivalue<0 )
            {
                Ivalue=0;
            }
        }
        if ( Ki==0 )
        {
            Ivalue=0;
        }
        //D-factor
        /*Dvalue=((error-olderror)*(5*Kd/dt));
        olderror=error;
        Dvalue=0;*/
        //PID
        Dvalue=0;
        PID=(Pvalue+Ivalue-Dvalue);
        if ( PID>100 )
        {
            PID=100;
        }
        else
        {
            if ( PID<0 )
            {
                PID=0;
            }
        }
        PIDold=PID;
        /* EasyCODE - */
        PID=PID*235.76;
        if ( PID>23000 )
        {
            PID=23000;
        }
        else
        {
            if ( PID<500 )
            {
                PID=500;
            }
        }
        PIDset=(unsigned int)PID;
    }
    else
    {
        PIDset=0;
    }
}

float currin   = 0.0;   // x[n]
float lastin   = 0.0;   // x[n-1]
float lastin2  = 0.0;   // x[n-2]
float currout  = 0.0;   // y[n]
float lastout  = 0.0;   // y[n-1]
float lastout2 = 0.0;   // y[n-2]

float biquad(float in)
{
    lastin2 =lastin;
    lastin=currin;
    currin=in;
    lastout2=lastout;
    lastout=currout;
    currout=a0*currin + a1*lastin + a2*lastin2 - b1*lastout - b2*lastout2;
    return currout;
}

void __attribute__((interrupt, auto_psv)) _ADCInterrupt(void)
{
    /* ReadADC */
    _BEMF_C = ADCBUF0; 			// Read AN0 conversion result
    ADSTATbits.P0RDY=0; 		// Clear the data is ready in buffer bits
    
    _BEMF_B = ADCBUF2; 			// Read AN2 conversion result
    ADSTATbits.P1RDY=0; 		// Clear the data is ready in buffer bits
    
    _BEMF_A = ADCBUF4; 			// Read AN4 conversion result
    ADSTATbits.P2RDY=0; 		// Clear the data is ready in buffer bits
    
    _CURRENT = ADCBUF9; 			// Read AN4 conversion result
    ADSTATbits.P4RDY=0; 		// Clear the data is ready in buffer bits
    /* BUS */
    if ( BEMFMODE==1 )
    {
        _BEMF_BUS=(_BEMF_A+_BEMF_B+_BEMF_C)/3;
    }
    else
    {
        _BEMF_BUS=_BEMF_BUS_LOW;
    }
    if ( PR3>=40000 && state>=3 )
    {
        if ( errorcount>errorthreshold )
        {
            state=0;
            MDC=0;
            Setpoint=0;
            SetpointOld=0;
        }
        else
        {
            errorcount++;
        }
		return;
    }

	const unsigned int * commuteCheckFirst[] = { &_BEMF_A,	&_BEMF_BUS, &_BEMF_B,	&_BEMF_BUS, &_BEMF_C,	&_BEMF_BUS };
	const unsigned int * commuteCheckSecond[] = {&_BEMF_BUS,&_BEMF_C,	&_BEMF_BUS, &_BEMF_A,	&_BEMF_BUS, &_BEMF_B };

	if (state==3 || state==2)
	{
		if (BEMFSTARTCOUNT>=6)
		{
			if ( DEMAG==0 )
			{
				if ( BEMFSTARTCOUNT>=5000 )
				{
					CDELAY=2;
				}
				else
				{
					BEMFSTARTCOUNT++;
					CDELAY=1;
				}
				/* BEMF */

				if (*commuteCheckFirst[Step-1] >= *commuteCheckSecond[Step-1])
				{
					state = 4;
					Step++;
					if (Step == 7) Step = 1;
					DEMAGS=0;
					TIMER3ZC = TMR2;
					TMR2 = 0;
					TMR3 = 0;
					RPMpulse = TIMER3ZC;
					TIMER3ZC = TIMER3ZC>>CDELAY;
					PR3 = TIMER3ZC;
				}
			}
			else
			{
				if (*commuteCheckFirst[Step-1] < *commuteCheckSecond[Step-1])
				{
					DEMAG = 0;
					DEMAGS = 0;
				}
				else
				{
					DEMAG--;
				}
			}
		}
		else
		{
			if ( DEMAG==0 )
			{
				if (*commuteCheckFirst[Step-1] >= *commuteCheckSecond[Step-1])
				{
					BEMFSTARTCOUNT++;
					TMR2=0;
				}
			}
			else
			{
				if (*commuteCheckFirst[Step-1] < *commuteCheckSecond[Step-1])
				{
					DEMAG=0;
					DEMAGS=0;
				}
				else
				{
					DEMAG--;
				}
			}
		}
	}
	errorcount=0;
    IFS0bits.ADIF=0;
    ADSTAT=0;
}

void openloop_pwm()
{
	IOCON1bits.OVRENH = (Step == 0 || Step == 2 || Step == 3) ? 1 : 0; //_1OVRENH[Step];
	IOCON2bits.OVRENH = (Step == 0 || Step == 4 || Step == 5) ? 1 : 0; //_2OVRENH[Step];
	IOCON3bits.OVRENH = (Step == 0 || Step == 1 || Step == 6) ? 1 : 0; //_3OVRENH[Step];

	IOCON1bits.OVRENL = 1; //_1OVRENL[Step];
	IOCON2bits.OVRENL = 1; //_2OVRENL[Step];
	IOCON3bits.OVRENL = 1; //_3OVRENL[Step];

	IOCON1bits.OVRDAT1 = 0; //_1OVRDAT1[Step];
	IOCON2bits.OVRDAT1 = 0; //_2OVRDAT1[Step];
	IOCON3bits.OVRDAT1 = 0; //_3OVRDAT1[Step];

	IOCON1bits.OVRDAT0 = (Step == 5 || Step == 6) ? 1 : 0; //_1OVRDAT0[Step];
	IOCON2bits.OVRDAT0 = (Step == 1 || Step == 2) ? 1 : 0; //_2OVRDAT0[Step];
	IOCON3bits.OVRDAT0 = (Step == 3 || Step == 4) ? 1 : 0; //_3OVRDAT0[Step];
}

void bemfloop_pwm()
{
	IOCON1bits.OVRENH = IOCON1bits.OVRENL = (Step == 2 || Step == 3) ? 0 : 1;
	IOCON1bits.OVRDAT1 = 0;
	IOCON1bits.OVRDAT0 = (Step == 5 || Step == 6) ? 1 : 0;

	IOCON2bits.OVRENH = IOCON2bits.OVRENL = (Step == 4 || Step == 5) ? 0 : 1;
	IOCON2bits.OVRDAT1 = 0;
	IOCON2bits.OVRDAT0 = (Step == 1 || Step == 2) ? 1 : 0;

	IOCON3bits.OVRENH = IOCON3bits.OVRENL = (Step == 6 || Step == 1) ? 0 : 1;
	IOCON3bits.OVRDAT1 = 0;
	IOCON3bits.OVRDAT0 = (Step == 3 || Step == 4) ? 1 : 0;
}

void __attribute__((interrupt, auto_psv)) _T3Interrupt(void)
{
    OLED^=1;
	if (state == 0) stage_align();
	else if (state == 1) openloop();

    switch ( state )
    {
    case 2 :
        /* EasyCODE ( 0 
        Openloop with ADC */
        DEMAG=10;
        DEMAGS=1;
        /* EasyCODE ( 0 
        PWM States new */
        Step++;
        if ( Step>6 )
        {
            Step=1;
            GLED=1;
        }
        else
        {
            GLED=0;
        }

		
//		const unsigned char _1OVRENH[] = {1,	1,0,0,1,1,1};
//		const unsigned char _1OVRENL[] = {1,	1,1,1,1,1,1};
//		const unsigned char _1OVRDAT1[] = {0,	0,0,0,0,0,0};
//		const unsigned char _1OVRDAT0[] = {0,	0,0,0,0,1,1};

//		const unsigned char _2OVRENH[] = {1,	1,1,1,0,0,1};
//		const unsigned char _2OVRENL[] = {1,	1,1,1,1,1,1};
//		const unsigned char _2OVRDAT1[] = {0,	0,0,0,0,0,0};
//		const unsigned char _2OVRDAT0[] = {0,	1,1,0,0,0,0};

//		const unsigned char _3OVRENH[] = {1,	0,1,1,1,1,0};
//		const unsigned char _3OVRENL[] = {1,	1,1,1,1,1,1};
//		const unsigned char _3OVRDAT1[] = {0,	0,0,0,0,0,0};
//		const unsigned char _3OVRDAT0[] = {0,	0,0,1,1,0,0};


		openloop_pwm();


		if (Step == 0)
		{
            MDC=0;
            state=0;
        }

        /* EasyCODE ) */
        ADCPC0bits.TRGSRC0=0b00100;//Sample MC 
        ADCPC0bits.TRGSRC1=0b00100;//Sample MB
        ADCPC1bits.TRGSRC2=0b00100;//Sample MA
        ADCPC2bits.TRGSRC4=0b01110; //Current AN9 0b01110 = PWM Generator 1 secondary trigger selected
        
        ADCPC0bits.IRQEN1=0;// Interrupt Request Enable 1 bit
        //1 = Enable IRQ generation when requested conversion of channels AN3 and AN2 is completed
        //0 = IRQ is not generated
        
        ADCPC0bits.IRQEN0=0;// Interrupt Request Enable 0 bit
        //1 = Enable IRQ generation when requested conversion of channels AN1 and AN0 is completed
        //0 = IRQ is not generated
        
        ADCPC1bits.IRQEN2=1;// Interrupt Request Enable 2 bit(2)
        //1 = Enable IRQ generation when requested conversion of channels AN5 and AN4 is completed
        //0 = IRQ is not generated
        
        ADCPC2bits.IRQEN4=0;// Interrupt Request Enable 2 bit(2)
        
        //1 = Enable IRQ generation when requested conversion of channels AN9 and AN8 is completed
        //0 = IRQ is not generated
        /* EasyCODE - */
        PIDStartCounter=0;
        /* EasyCODE ) */
        break;
    case 3 :
        /* EasyCODE ( 0 
        Measuring BEMF */
        /* EasyCODE ) */
        break;
    case 4 :
        /* EasyCODE ( 0 
        Wait for commutation */
        DEMAGtime=RPMpulse;
        DEMAGtime=DEMAGtime>>5;
        if ( DEMAGtime<=2 )
        {
            DEMAGtime=2;
        }
        else
        {
            if ( DEMAGtime>=10 )
            {
                DEMAGtime=10;
            }
        }
        DEMAG=(unsigned char)(DEMAGtime);
        DEMAGS=1;
        state=3;
        TMR3=0;

		bemfloop_pwm();

		ADCPC0bits.TRGSRC0=0b00100;//Sample MC
		ADCPC0bits.TRGSRC1=0b00100;//Sample MB
		ADCPC1bits.TRGSRC2=0b00100;//Sample MA

		FCLCON1bits.CLMOD = (Step == 2 || Step == 3) ? CL_ON : 0; //MA
		FCLCON2bits.CLMOD = (Step == 4 || Step == 5) ? CL_ON : 0; //MB
		FCLCON3bits.CLMOD = (Step == 1 || Step == 6) ? CL_ON : 0; //MC

		// 3375 - 190 ; 3294 : 190
		if (Step == 0)
		{
            //MA
            IOCON1bits.OVRENH=1;//Override Enable for PWM1H Pin bit 0=PWM 1= OVD
            IOCON1bits.OVRENL=1; //Override Enable for PWM1L Pin bit 0=PWM 1= OVD
            IOCON1bits.OVRDAT1=0; //PWM1H
            IOCON1bits.OVRDAT0=0; //PWM1L

            //MB
            IOCON2bits.OVRENH=1;//Override Enable for PWM2H Pin bit 0=PWM 1= OVD
            IOCON2bits.OVRENL=1; //Override Enable for PWM2L Pin bit 0=PWM 1= OVD
            IOCON2bits.OVRDAT1=0; //PWM2H
            IOCON2bits.OVRDAT0=0; //PWM2L


            //MC
            IOCON3bits.OVRENH=1;//Override Enable for PWM3H Pin bit 0=PWM 1= OVD
            IOCON3bits.OVRENL=1; //Override Enable for PWM3L Pin bit 0=PWM 1= OVD
            IOCON3bits.OVRDAT1=0; //PWM3H
            IOCON3bits.OVRDAT0=0; //PWM3L

            //A/D Trigger Select
            ADCPC0bits.TRGSRC0=0b00000;//Sample MC
            ADCPC0bits.TRGSRC1=0b00000;//Sample MB
            ADCPC1bits.TRGSRC2=0b00000;//Sample MA

            FCLCON1bits.CLMOD=CL_ON; //MA
            FCLCON2bits.CLMOD=CL_ON; //MB
            FCLCON3bits.CLMOD=CL_ON; //MC
            //Current-Limit Mode Enable bit for PWM Generator # bit
            //1 = Current-limit function is enabled
            //0 = Current-limit function is disabled

            MDC=0;
            state=0;
		}

        /* EasyCODE ) */
        PR3=20000;
        /* EasyCODE ) */
        if ( Step==1 )
        {
            GLED=1;
        }
        else
        {
            GLED=0;
        }
        break;
    default:
        state=0;
        MDC=0;
        break;
    }
    IFS0bits.T3IF = 0; //Clear timer flag for next period
}

/* EasyCODE ) */
/* EasyCODE ( 0 
_PWMSpEventMatchInterrupt(void) */
/* EasyCODE F */
void __attribute__((interrupt, auto_psv)) _PWMSpEventMatchInterrupt(void)
{
    if ( state>=3 )
    {
        /* EasyCODE ( 0 
        PWM Cycle */
        if ( PIDStartDelay==0 )
        {
        }
        else
        {
            if ( PIDStartCounter>=(PIDStartDelay/2) )
            {
                if ( PIDStartCounter<PIDStartDelay )
                {
                    PIDStartCounter++;
                }
                if ( PIDset==0 //|| RPM==0
                   )
                {
                    if ( BEMFMODE==1 )
                    {
                        TRISAbits.TRISA1 = 0; //MAA
                        TRISCbits.TRISC13=0; //MBA
                        TRISCbits.TRISC0=0; //MCA
                        STRIG1=PWMopenloop/2;
                        TRIG1=(PWMopenloop-200);
                        TRIG2=(PWMopenloop-200);
                        TRIG3=(PWMopenloop-200);
                        MDC=PWMopenloop;
                    }
                    else
                    {
                        TRISAbits.TRISA1 = 1; //MAA
                        TRISCbits.TRISC13=1; //MBA
                        TRISCbits.TRISC0=1; //MCA
                        STRIG1=PWMopenloop/2;
                        TRIG1=18859;
                        TRIG2=18859;
                        TRIG3=18859;
                        MDC=PWMopenloop;
                    }
                }
                else
                {
                    if ( PIDset>16000 && BEMFMODE==0 )
                    {
                        BEMFMODE=1;
                        /* EasyCODE - */
                        TRISAbits.TRISA1 = 0; //MAA
                        TRISCbits.TRISC13=0; //MBA
                        TRISCbits.TRISC0=0; //MCA
                    }
                    else
                    {
                        if ( PIDset<6000 && BEMFMODE==1 )
                        {
                            BEMFMODE=0;
                            /* EasyCODE - */
                            TRISAbits.TRISA1 = 1; //MAA
                            TRISCbits.TRISC13=1; //MBA
                            TRISCbits.TRISC0=1; //MCA
                        }
                    }
                    if ( BEMFMODE==1 )
                    {
                        /* EasyCODE ( 0 
                        Update PWM & ADC Trigger */
                        if ( PIDset>oldPIDset )
                        {
                            if ( PIDset>17782 )
                            {
                                MDC=PIDset;
                                SDC1=PIDset;
                                /* EasyCODE - */
                                STRIG1=((PIDset/3)+250);
                                TRIG1=17682;
                                TRIG2=17682;
                                TRIG3=17682;
                            }
                            else
                            {
                                MDC=PIDset;
                                SDC1=PIDset;
                                /* EasyCODE - */
                                STRIG1=((PIDset/3)+250);
                                TRIG1=(PIDset-100);
                                TRIG2=(PIDset-100);
                                TRIG3=(PIDset-100);
                            }
                        }
                        else
                        {
                            if ( PIDset>17782 )
                            {
                                STRIG1=((PIDset/3)+250);
                                TRIG1=17682;
                                TRIG2=17682;
                                TRIG3=17682;
                                /* EasyCODE - */
                                MDC=PIDset;
                                SDC1=PIDset;
                            }
                            else
                            {
                                STRIG1=((PIDset/3)+250);
                                TRIG1=(PIDset-100);
                                TRIG2=(PIDset-100);
                                TRIG3=(PIDset-100);
                                /* EasyCODE - */
                                MDC=PIDset;
                                SDC1=PIDset;
                            }
                        }
                        /* EasyCODE ) */
                        oldPIDset=PIDset;
                    }
                    else
                    {
                        STRIG1=((PIDset/3)+250);
                        TRIG1=18859;
                        TRIG2=18859;
                        TRIG3=18859;
                        //MDC=PWMopenloop;
                        /* EasyCODE - */
                        MDC=PIDset;
                        SDC1=PIDset;
                        //oldPIDset=PIDset;
                    }
                }
            }
            else
            {
                PIDStartCounter++;
                if ( BEMFMODE==1 )
                {
                    TRISAbits.TRISA1 = 0; //MAA
                    TRISCbits.TRISC13=0; //MBA
                    TRISCbits.TRISC0=0; //MCA
                    STRIG1=PWMopenloop/2;
                    TRIG1=(PWMopenloop-200);
                    TRIG2=(PWMopenloop-200);
                    TRIG3=(PWMopenloop-200);
                    MDC=PWMopenloop; //PWM 20%
                    PIDset=PWMopenloop;
                    oldPIDset=PIDset;
                }
                else
                {
                    TRISAbits.TRISA1 = 1; //MAA
                    TRISCbits.TRISC13=1; //MBA
                    TRISCbits.TRISC0=1; //MCA
                    STRIG1=PWMopenloop/2;
                    TRIG1=18859;
                    TRIG2=18859;
                    TRIG3=18859;
                    MDC=PWMopenloop; //PWM 20%
                    PIDset=PWMopenloop;
                    oldPIDset=PIDset;
                }
            }
        }
        /* EasyCODE ) */
    }
    IFS3bits.PSEMIF = 0;
}

void __attribute__((interrupt, auto_psv))  _SI2C1Interrupt(void)
{
    static unsigned char MSB=0;
    static unsigned char LSB=0;
    static unsigned int Dutycycle=0;

    if ( I2C1STATbits.D_A==0 )
    {
        _i2cState=0;
    }
    switch ( _i2cState )
    {
    case 0 :
        _i2cAddress=I2C1RCV;
        _i2cAddress=(_i2cAddress>>1);
        _i2cState=1;
        break;
    case 1 :
        MSB=I2C1RCV;
        _i2cState=2;
        break;
    case 2 :
        LSB=I2C1RCV;
        /* EasyCODE - */
        Dutycycle=MSB;
        Dutycycle=(Dutycycle<<8);
        Dutycycle+=LSB;
        _i2cState=0;
        _i2cAddress=0;
        Setpoint=Dutycycle;
        _i2cTimeout=0;
        if ( Setpoint<MinSetpoint )
        {
            if ( Setpoint==SetpointOld )
            {
                Setpoint=0;
                /* EasyCODE - */
                SetpointOld=Setpoint;
            }
            else
            {
                Setpoint=SetpointOld;
                SetpointOld=Dutycycle;
            }
        }
        else
        {
            if ( Setpoint>9500 )
            {
                Setpoint=9500;
            }
            SetpointOld=Setpoint;
        }                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    
        break;
    default:
        _i2cState=0;
        break;
    }
    _SI2C1IF=0;
}

void __attribute__((interrupt, auto_psv)) _T1Interrupt(void)
{
    TIMER=1;
    startcalculation=1;
    IFS0bits.T1IF = 0; //Clear timer flag for next period
}

void __attribute__((interrupt, auto_psv)) _U1RXInterrupt(void)
{
    if ( U1STAbits.OERR )
    {
        U1STAbits.OERR=0;
    }
    _U1RXIF=0;
}

//Oscillator Fail Error trap routine
void __attribute__((interrupt, auto_psv)) _OscillatorFail(void)
{
    asm("RESET");
}

//Address Error trap routine
void __attribute__((interrupt, auto_psv)) _AddressError(void)
{
    asm("RESET");
}

//Stack Error trap routine
void __attribute__((interrupt, auto_psv)) _StackError(void)
{
    asm("RESET");
}

//Math (Arithmetic) Error trap routine
void __attribute__((interrupt, auto_psv)) _MathError(void)
{
    asm("RESET");
}
